//
// Created by yunnan xu on 2020/12/11.
//

#include "GJK.h"
#include "Convex.h"
#include "Basic.h"
#include "gtx/vector_angle.hpp"
#include "gtx/projection.hpp"
#include "RigidBody.h"
#include <memory>

namespace Phy {

    namespace GJK {

        /////////////////////////////////////////////////////////////////////////////////////////////
        // GJK 核心算法
        /////////////////////////////////////////////////////////////////////////////////////////////
        bool GJKCollision(const std::shared_ptr<Phy::RigidBody>& r1, const std::shared_ptr<Phy::RigidBody>& r2, Simplex & gjk_simplex)
        {
            Simplex simplex;
            glm::vec3 dir = glm::vec3(1, 0, 0);
            glm::vec3 support_point = r1->convex->GetSupport(dir).pos - r2->convex->GetSupport(-dir).pos;

            dir = - support_point;
            while(true)
            {
                // support_point 在原始的视图中，是凸包A中的一个点 -> 凸包B中的一个点的 向量，在 闵可夫斯基 视图中，是闵可夫斯基差集的一个点
                auto sp = SupportPointInGJK(r1->convex, r2->convex, dir);
                if (glm::dot(sp.minkowski_pos, dir) < 0)
                {
                    return false;
                }

                simplex.AddPoint(sp);

                if(simplex.UpdateSimplex(dir))
                {
                    gjk_simplex = simplex;
                    return true;
                }
            }
        }

        SupportPointInGJK::SupportPointInGJK(const std::shared_ptr<Convex>& c1, const std::shared_ptr<Convex>& c2, glm::vec3 dir)
        {
            a_origin_wpos = c1->GetSupport(dir).pos;
            b_origin_wpos = c2->GetSupport(-dir).pos;
            minkowski_pos = a_origin_wpos - b_origin_wpos;
        }

        FaceInGJK::FaceInGJK(SupportPointInGJK p1, SupportPointInGJK p2, SupportPointInGJK p3) : pos1(p1), pos2(p2), pos3(p3)
        {
            glm::vec3 dir12 = pos2.minkowski_pos - pos1.minkowski_pos;
            glm::vec3 dir13 = pos3.minkowski_pos - pos1.minkowski_pos;
            norm = glm::normalize(glm::cross(dir12, dir13));
        }

        EdgeInGJK::EdgeInGJK(SupportPointInGJK p1, SupportPointInGJK p2): pos1(p1), pos2(p2)
        {
            mid_point = (p1.minkowski_pos + p2.minkowski_pos) * 0.5f;
        }

        /*
         *  EPA 算法的流程：
         *  1. 算法的输入是 GJK 计算出来的 包含原点的四面体 Simplex、Convex1 & Convex2 两个凸包
         *  2. 定义 face_vector 和 normal_vector ，表示在EPA算法执行过程中，拓展出来的图形poly，前者3个vec3，后者1个vec3
         *  3. 迭代开始，限制迭代的轮数不超过预设数
         *  4.      查找 face_vector 里，距离原点最近的面 closest_face，最近距离为 min_dist，最近面的法线向外方向是 closest_face_normal
         *  5.      计算 闵可夫斯基差集 在 closest_face_normal 方向上的支撑点 p ，如果 OP 在 closest_face_normal 方向上的投影 == min_dist ，则记录下最后一次跳转到 15 -> 计算碰撞信息
         *  6.      接下来，就是处理把p点添加到 poly 里面的操作了！需要计算出来哪些边需要和p点连接，组成新的三角形面；新的图形中，包含了哪些已有的face，需要把他们去掉！
         *  7.      定义 new_edge 集合, 表示将要和p点组成新三角形的边, 也就是朝着p点的face组成的面的集合的外围轮廓
         *  8.      遍历 face vector 中的每个面向p点的 face （face_normal · OP > 0） （如果面向了p，那么说明需要去掉这个面）
         *  9.          face 有 三个边，e1 e2 e3, 遍历每条边 e
         *  10.              如果 e已经在 new_edge 里面了：
         *                      说明 e 被 向 new_edge 里面 添加了2次，说明e的两个邻接三角形，都是朝向p的，也就是说e不是 “朝着p点的face组成的面的集合的外围轮廓”，而是内部边。从 new_edge 中删除e
         *  11.              否则：
         *                      把 e 添加到 new_edge 里面
         *  12.         从 face_vector 中 删除 face
         *  13.     这时候，我们拿到了 将要和p一起被添加到 face_vector、normal_vector 中的 新的的边集合 new_edge， 依次添加它们，更新顶点顺序为 逆时针（右手定则）。至此 poly 添加了 p，更新完毕；
         *  14.     回到 3，开始下一轮迭代
         *
         *  15. 计算出原点在最近三角形上面的投影的位置，还有相对于三个顶点权重 u,v,w   我们使用这个权重来把坐标从 闵可夫斯基空间 转换 到 世界空间。
         *      最终结果 = u * p1_world_pos + v * p2_world_pos + w * p3_world_pos
         *
         * */

        void barycentric(const glm::vec3 &p,const glm::vec3 &a,const glm::vec3 &b,const glm::vec3 &c,float *u,float *v,float *w) {
            // code from Crister Erickson's Real-Time Collision Detection
            glm::vec3 v0 = b - a;
            glm::vec3 v1 = c - a;
            glm::vec3 v2 = p - a;
            float d00 = glm::dot(v0, v0);
            float d01 = glm::dot(v0, v1);
            float d11 = glm::dot(v1, v1);
            float d20 = glm::dot(v2, v0);
            float d21 = glm::dot(v2, v1);
            float denom = d00 * d11 - d01 * d01;
            *v = (d11 * d20 - d01 * d21) / denom;
            *w = (d00 * d21 - d01 * d20) / denom;
            *u = 1.0f - *v - *w;
        }

        void GenerateCollisionInfo(const FaceInGJK & closest_face, CollisionInfo & res)
        {
            const float distance_from_origin = glm::dot(closest_face.pos1.minkowski_pos, closest_face.norm);
            float bary_u,bary_v,bary_w;

            barycentric(closest_face.norm * distance_from_origin,
                        closest_face.pos1.minkowski_pos,
                        closest_face.pos2.minkowski_pos,
                        closest_face.pos3.minkowski_pos,
                        &bary_u,
                        &bary_v,
                        &bary_w);

            res.collision = true;
            res.rb1_pos = res.rb2_pos = bary_u * closest_face.pos1.a_origin_wpos + bary_v * closest_face.pos2.a_origin_wpos + bary_w * closest_face.pos3.a_origin_wpos;
            res.normal = -closest_face.norm;
        }

        CollisionInfo EPACollision(const std::shared_ptr<Phy::RigidBody>& r1, const std::shared_ptr<Phy::RigidBody>& r2)
        {
            Simplex simplex = {};
            bool gjk_collision_res = GJKCollision(r1, r2, simplex);
            if (!gjk_collision_res) return {};

            std::vector<FaceInGJK> face_vector;
            std::vector<SupportPointInGJK> init_points = simplex.GetPoints();

            std::vector<FaceInGJK> temp_face_vector;

            face_vector.emplace_back(FaceInGJK(init_points[0], init_points[3], init_points[1]));
            face_vector.emplace_back(FaceInGJK(init_points[0], init_points[2], init_points[3]));
            face_vector.emplace_back(FaceInGJK(init_points[3], init_points[2], init_points[1]));
            face_vector.emplace_back(FaceInGJK(init_points[0], init_points[1], init_points[2]));

            PHYLOGI("EPA algo init_points[0] = (%f, %f, %f), init_points[1] = (%f, %f, %f), init_points[2] = (%f, %f, %f), init_points[3] = (%f, %f, %f)",
                    init_points[0].minkowski_pos.x, init_points[0].minkowski_pos.y, init_points[0].minkowski_pos.z,
                    init_points[1].minkowski_pos.x, init_points[1].minkowski_pos.y, init_points[1].minkowski_pos.z,
                    init_points[2].minkowski_pos.x, init_points[2].minkowski_pos.y, init_points[2].minkowski_pos.z,
                    init_points[3].minkowski_pos.x, init_points[3].minkowski_pos.y, init_points[3].minkowski_pos.z
            );


            PHYLOGI("EPA algo normal_vector[0] = (%f, %f, %f), normal_vector[1] = (%f, %f, %f), normal_vector[2] = (%f, %f, %f), normal_vector[3] = (%f, %f, %f)",
                    face_vector[0].norm.x, face_vector[0].norm.y, face_vector[0].norm.z,
                    face_vector[1].norm.x, face_vector[1].norm.y, face_vector[1].norm.z,
                    face_vector[2].norm.x, face_vector[2].norm.y, face_vector[2].norm.z,
                    face_vector[3].norm.x, face_vector[3].norm.y, face_vector[3].norm.z
            );

            for (int iter_time = 0; iter_time < MAX_EPA_ITER_TIMES; iter_time ++)
            {
                int closest_face_index = -1;
                float min_dist = FLOAT_MAX;
                int poly_now_face_num = face_vector.size();

                temp_face_vector.clear();

                for (int face_id = 0; face_id < poly_now_face_num; face_id ++)
                {
                    glm::vec3 one_face_point = face_vector[face_id].pos1.minkowski_pos;

                    float ori_dist = glm::dot(one_face_point, face_vector[face_id].norm);
                    if (ori_dist < min_dist)
                    {
                        min_dist = ori_dist;
                        closest_face_index = face_id;
                    }
                }

                glm::vec3 out_norm = face_vector[closest_face_index].norm;
                // glm::vec3 p = r1->convex->GetSupport(out_norm).pos - r2->convex->GetSupport(-out_norm).pos;

                SupportPointInGJK p(r1->convex, r2->convex, out_norm);

                PHYLOGI("   EPA algo iter %d, poly_now_face_num = %d, closest_face_index = %d, p = (%f, %f, %f), norm = (%f, %f, %f)", iter_time, poly_now_face_num, closest_face_index,
                        p.minkowski_pos.x, p.minkowski_pos.y, p.minkowski_pos.z, out_norm.x, out_norm.y, out_norm.z);

                float op_norm_length = glm::dot(p.minkowski_pos, out_norm);

                if (abs(op_norm_length - min_dist) < EPA_EPS)
                {
                    CollisionInfo res;
                    GenerateCollisionInfo(face_vector[closest_face_index], res);
                    return res;
                }

                std::vector<EdgeInGJK> new_edge;

                for (int face_id = 0; face_id < poly_now_face_num; face_id ++)
                {
                    glm::vec3 norm = face_vector[face_id].norm;

                    PHYLOGI("       EPA algo norm = (%f, %f, %f), glm::dot(norm, p) = %f", norm.x, norm.y, norm.z, glm::dot(norm, p.minkowski_pos));

                    if (glm::dot(norm, p.minkowski_pos) > 0)  // 朝向p的面
                    {
                        EdgeInGJK edges[3] = { {face_vector[face_id].pos1, face_vector[face_id].pos2},
                                             {face_vector[face_id].pos2, face_vector[face_id].pos3},
                                             {face_vector[face_id].pos3, face_vector[face_id].pos1} };

                        for (auto & edge : edges)
                        {
                            int same_edge_id = -1;

                            for (int i = 0; i < new_edge.size(); i ++)
                            {
                                if (glm::distance(new_edge[i].mid_point, edge.mid_point) < 0.00001f)
                                {
                                    same_edge_id = i;
                                    break;
                                }
                            }

                            // 在这里找到了一样的边
                            if (same_edge_id > 0)
                            {
                                new_edge[same_edge_id] = new_edge[new_edge.size() - 1];
                                new_edge.pop_back();

                                PHYLOGI("               EPA algo delete edge (%f, %f, %f) - (%f, %f, %f)", edge.pos1.minkowski_pos.x, edge.pos1.minkowski_pos.y, edge.pos1.minkowski_pos.z, edge.pos2.minkowski_pos.x, edge.pos2.minkowski_pos.y, edge.pos2.minkowski_pos.z);
                            }
                            // 暂时还没找到
                            else
                            {
                                new_edge.emplace_back(edge);
                                PHYLOGI("               EPA algo add edge (%f, %f, %f) - (%f, %f, %f)", edge.pos1.minkowski_pos.x, edge.pos1.minkowski_pos.y, edge.pos1.minkowski_pos.z, edge.pos2.minkowski_pos.x, edge.pos2.minkowski_pos.y, edge.pos2.minkowski_pos.z);
                            }
                        }
                    }
                    else
                    {
                        temp_face_vector.emplace_back(face_vector[face_id]);
                    }
                }

                face_vector = temp_face_vector;

                PHYLOGI("   EPA algo new_edge.size() = %d, face_vector.size() = %d", (int)new_edge.size(), (int)face_vector.size());

                // 到这里，我们已经得到了 “朝着p点的face组成的面的集合的外围轮廓” new_edge
                for (auto & seg : new_edge) {
                    face_vector.emplace_back(FaceInGJK(p, seg.pos1, seg.pos2));
                }

                poly_now_face_num = face_vector.size();
                // 修正顶点顺序 和 法线方向, 保证新的几何体的所有面一定都是向外的
                for (int face_id = 0; face_id < poly_now_face_num; face_id ++)
                {
                    // 先保证法向的正确性
                    if (glm::dot(face_vector[face_id].pos1.minkowski_pos, face_vector[face_id].norm) < 0)
                    {
                        face_vector[face_id].norm = -face_vector[face_id].norm;
                    }

                    // 再判断顶点顺序的正确性，如果反向了，那么需要修正
                    glm::vec3 dir12 = face_vector[face_id].pos2.minkowski_pos - face_vector[face_id].pos1.minkowski_pos;
                    glm::vec3 dir13 = face_vector[face_id].pos3.minkowski_pos - face_vector[face_id].pos1.minkowski_pos;
                    auto norm = glm::normalize(glm::cross(dir12, dir13));

                    if (glm::dot(norm, face_vector[face_id].norm) < 0)
                    {
                        auto temp = face_vector[face_id].pos3;
                        face_vector[face_id].pos3 = face_vector[face_id].pos2;
                        face_vector[face_id].pos2 = temp;
                    }
                }
            }
            return {};
        }


        /////////////////////////////////////////////////////////////////////////////////////////////
        // Simplex 的实现
        /////////////////////////////////////////////////////////////////////////////////////////////

        const std::vector<SupportPointInGJK> & Simplex::GetPoints()
        {
            return points;
        }

        void Simplex::AddPoint(SupportPointInGJK p)
        {
            points.emplace_back(p);
        }

        bool Simplex::UpdateSimplex(glm::vec3 & dir)
        {
            int dimension = points.size();

            /*
             *  实际上，只有 UpdateTetrahedron 这个函数可能返回 true，其他的都是一定返回false
             * */
            if (dimension == 1)
            {
                return false;
            }
            else if (dimension == 2)
            {
                return UpdateLine(dir);
            }
            else if (dimension == 3)
            {
                return UpdateTriangle(dir);
            }
            else if (dimension == 4)
            {
                return UpdateTetrahedron(dir);
            }
            else
            {
                assert(false);
            }
        }

        bool Simplex::UpdateLine(glm::vec3 & dir)
        {
            // a 是最后添加的点
            SupportPointInGJK a = points[1];
            SupportPointInGJK b = points[0];

            glm::vec3 ab = b.minkowski_pos - a.minkowski_pos;
            glm::vec3 ao = -a.minkowski_pos;

            if (glm::dot(ab, ao) > 0)
            {
                dir = glm::cross(glm::cross(ab, ao), ab);
                dir = glm::normalize(dir);
            }
            else
            {
                // 删除b
                points.erase(points.cbegin());
                dir = glm::normalize(ao);
            }

            return false;
        }

        bool Simplex::UpdateTriangle(glm::vec3 & dir)
        {
            // a 是最后添加的点
            SupportPointInGJK a = points[2];
            SupportPointInGJK b = points[1];
            SupportPointInGJK c = points[0];

            glm::vec3 ab = b.minkowski_pos - a.minkowski_pos;
            glm::vec3 ac = c.minkowski_pos - a.minkowski_pos;
            glm::vec3 ao = -a.minkowski_pos;
            glm::vec3 abc = glm::cross(ab, ac); // 垂直于abc三角形的平面的向量
            glm::vec3 h_ac = glm::cross(abc, ac); // abc平面中，垂直于ac向三角形外的向量
            glm::vec3 h_ab = glm::cross(ab, abc); // abc平面中，垂直于ab向三角形外的向量

            // 如果是在 ao 在 ac 边这个方向
            if (glm::dot(h_ac, ao) > 0)
            {

                if (glm::dot(ac, ao))
                {
                    // 删掉b
                    points.erase(points.cbegin() + 1);
                    dir = glm::cross(glm::cross(ac, ao), ac);
                    dir = glm::normalize(dir);
                }
                else
                {
                    // 删掉c
                    points.erase(points.cbegin());
                    return UpdateLine(dir);
                }
            }
            else
            {
                if (glm::dot(h_ab, ao) > 0)
                {
                    // 删掉c
                    points.erase(points.cbegin());
                    return UpdateLine(dir); // TODO: 检查这里是否可以 dir = glm::cross(glm::cross(ab, ao), ab);
                }
                else
                {
                    if (glm::dot(ao, abc) > 0)
                    {
                        dir = abc;
                    }
                    else
                    {
                        points.clear();
                        points.emplace_back(b);
                        points.emplace_back(c);
                        points.emplace_back(a);
                        dir = -abc;
                    }
                }
            }

            return false;
        }

        bool Simplex::UpdateTetrahedron(glm::vec3 & dir)
        {
            // a 是最后添加的点
            SupportPointInGJK a = points[3];
            SupportPointInGJK b = points[2];
            SupportPointInGJK c = points[1];
            SupportPointInGJK d = points[0];

            glm::vec3 ab = b.minkowski_pos - a.minkowski_pos;
            glm::vec3 ac = c.minkowski_pos - a.minkowski_pos;
            glm::vec3 ad = d.minkowski_pos - a.minkowski_pos;
            glm::vec3 ao = -a.minkowski_pos;

            glm::vec3 abc = glm::cross(ab, ac);
            glm::vec3 acd = glm::cross(ac, ad);
            glm::vec3 adb = glm::cross(ad, ab);

            if (glm::dot(abc, ao) > 0)
            {
                points.clear();
                points.push_back(c);
                points.push_back(b);
                points.push_back(a);
                return UpdateTriangle(dir);
            }

            if (glm::dot(acd, ao) > 0)
            {
                points.clear();
                points.push_back(d);
                points.push_back(c);
                points.push_back(a);
                return UpdateTriangle(dir);
            }

            if (glm::dot(adb, ao) > 0)
            {
                points.clear();
                points.push_back(b);
                points.push_back(d);
                points.push_back(a);
                return UpdateTriangle(dir);
            }

            return true;
        }
    }
}